#include <Servo.h>
#include <EEPROM.h>
const int numOfServo = 7;
Servo servo[numOfServo];
String comdata = "";                                  //comdata用来存储读取的字符串

int stretchDeg[numOfServo] = {0, 0, 0, 0, 0, 0, 0}, mark = 0;  //numdata是将comdata拆分之后的数字数组
int holdDeg[numOfServo] = {0, 0, 0, 0, 0, 0, 0};//依次为 拇指、食指、中指、无名指、小指、底盘、大拇指关节
int numdata[numOfServo] = {0, 0, 0, 0, 0, 0, 0};
int runstretchDeg[numOfServo] = {0, 0, 0, 0, 0, 0, 0};
int runholdDeg[numOfServo] = {0, 0, 0, 0, 0, 0, 0};

                        
int PTZ_Angle; //接收云台角度
int j; //接收到上位机数字个数
String sendmessage = ""; //发送给上位机的字符串初始化
int arr[14] = {0}; //存储读取到的舵机角度
void AngleInit(); //舵机角度初始化
int JudgeCommand();//判断上位机发送命令
void JudgeMark(int mark); //上位机命令处理函数

void setup() 
{
  AngleInit();
  Serial.begin(9600);
}

void loop() 
{
  JudgeMark(JudgeCommand());
}

void AngleInit()
{
    for(int i=0;i<5;i++)
    {
      runstretchDeg[i] = EEPROM.read(i);
    }
    runstretchDeg[6] = EEPROM.read(6);
  
  for(int i=7;i<12;i++)
    {
      runholdDeg[i-7] = EEPROM.read(i);
    }
    runholdDeg[6] = EEPROM.read(13);
    PTZ_Angle = EEPROM.read(5);
  
  for (int i = 0; i < numOfServo; i++) 
    {              
      servo[i].attach(i +2);
      servo[i].write(runstretchDeg[i]);
      delay(100);
      servo[5].write(PTZ_Angle); 
    }
    
  for (int i = 0; i < numOfServo; i++) 
    {              
      stretchDeg[i] = 0;
      holdDeg[i] = 0;
    }

}

int JudgeCommand()
{
  j=0;
  while (Serial.available() > 0) 
  {
    char c = Serial.read();
    if(c == 'z')
      return 5;
      
    else if (c == 'w') 
      return 1;
      
    else if (c == 'x')
      return 2;
      
    else if (c == 'y')
      return 3;
      
    else if (c == 'r')
      return 4;
      
    else if (c >= '0' && c <= '9' || c == ',') 
      {
        Serial.println(c);
        comdata += c;                                
        delay(2);
      }
      
    else 
      {
        comdata = "";
        while (Serial.read() >= 0);
      }
  }
}

void JudgeMark(int mark)
{
  if(mark == 5)
  {
     mark = 0;
     for (int i = 0; i < comdata.length(); i++)
      if (comdata[i] == ',') {
        j++;
      } else {
        numdata[j] = numdata[j] * 10 + (comdata[i] - '0');                              //
      }
    comdata = String("");                                                              //
    if (j == 6)
      {
      for (int i = 0; i < 5; i++) 
      {
        servo[i].write(map(numdata[i], 0, 100, runholdDeg[i], runstretchDeg[i]));                                                                            
      }
      if(numdata[5] <= 50)
      {
         servo[5].write(map(numdata[5], 0, 50, 0, PTZ_Angle));
      }
      else
      {
         servo[5].write(map(numdata[5], 50, 99, PTZ_Angle, 180)); 
      }
     
      servo[6].write(map(numdata[0], 0, 99, runholdDeg[6], runstretchDeg[6]));                       
      for (int i = 0; i < 6; i++) { 
        numdata[i] = 0; 
      }
       
      }
      if (j == 5)
      {
        for (int i = 0; i < 5; i++) {
          servo[i].write(map(numdata[i], 0, 99, runholdDeg[i], runstretchDeg[i])); 
                         
            //numdata[i] = 0;                                                                  //
        }
          servo[6].write(map(numdata[0], 0, 99, runholdDeg[6], runstretchDeg[6]));                       //大拇指关节舵机输出大拇指舵机的数据
          for (int i = 0; i < 6; i++) {
            numdata[i] = 0; 
        }
      }
  }
  else if (mark == 1) {
    mark = 0;
    for (int i = 0; i < comdata.length(); i++)
      if (comdata[i] == ',') {
        j++;
      } else {
        numdata[j] = numdata[j] * 10 + (comdata[i] - '0');                              
      }
    comdata = String("");                                                              
    if (j == 7)
      {
        for (int i = 0; i < 7; i++) {
        servo[i].write(map(numdata[i], 0, 99, 0, 180)); 
                                                                                      
      }       
      for (int i = 0; i < 7; i++) { 
        numdata[i] = 0; 
      }
       
      }
  }

else if (mark == 2) {          // 'x' 
    mark = 0;
    for (int i = 0; i < comdata.length(); i++)
      if (comdata[i] == ',') {
        j++;
      } else {
        stretchDeg[j] = stretchDeg[j] * 10 + (comdata[i] - '0');                           
      }
      Serial.print(comdata); 
    comdata = String("");                                                             
    if (j == 7)
      {
                        
      for(int i=0;i<7;i++)
      {
        EEPROM.write(i, stretchDeg[i]);
        Serial.print(stretchDeg[i]); 
      }
      for (int i = 0; i < 7; i++) {
        stretchDeg[i] = 0; 
      }
       
      }

  }
else if (mark == 3) {          // 'y'
    mark = 0;
    for (int i = 0; i < comdata.length(); i++)
      if (comdata[i] == ',') {
        j++;
      } else {
        holdDeg[j] = holdDeg[j] * 10 + (comdata[i] - '0');                              
      }
    comdata = String("");                                                              
    if (j == 7)
      {
                     
      for(int i=7;i<14;i++)
      {
        EEPROM.write(i, holdDeg[i-7]);
        Serial.print(holdDeg[i-7]);
        Serial.print(" ");
      }
      
      for (int i = 0; i < 7; i++) { 
        holdDeg[i] = 0; 
      }
       
      }

  }
else if (mark == 4) {       // 'r' 
    mark = 0;
    for (int i = 0; i < comdata.length(); i++)
      if (comdata[i] == ',') {
        j++;
      } else {
        numdata[j] = numdata[j] * 10 + (comdata[i] - '0');                             
      }
    comdata = String("");                                                          
    if (j == 7)
      {
                     
      for(int i=0;i<7;i++)
      {
        arr[i] = EEPROM.read(i);

      }
      for(int i=7;i<14;i++)
      {
        arr[i] = EEPROM.read(i);

      }
      for(int i=0;i<14;i++)
      {
        Serial.println(arr[i]);
      }
      

      for(int i=0;i<14;i++)
      {
        sendmessage = sendmessage + String(arr[i]) + ',';    
      }
      Serial.write("read");
      Serial.println(sendmessage);
      

      sendmessage = "";
      for (int i = 0; i < 7; i++) { 
        numdata[i] = 0; 
      }
       
      }

  }
}
